/* exponencial smoothing */
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

typedef struct
{
    int32_t  y_prev, y_sum;
    int16_t  s;
    int16_t  s_prev;
    int32_t  n;
} Filter;

#define dbg printf
Filter MofM_filt = {0};

#define min(a,b)     (((a)<(b))?(a):(b))
#define max(a,b)     (((a)>(b))?(a):(b))

int32_t floorlog2(uint32_t n) {
    int pos = 0;
    if (n == 0) {
        return -1;
    }
    if (n >= 1<<16) {
        n >>= 16; pos += 16; 
    }
    if (n >= 1<< 8) { 
        n >>=  8; pos +=  8; 
    }
    if (n >= 1<< 4) { 
        n >>=  4; pos +=  4; 
    }
    if (n >= 1<< 2) {
        n >>=  2; pos +=  2; 
    }
    if (n >= 1<< 1) {
        pos +=  1; 
    }
    return pos;
}

static int32_t order(int32_t n)
{
    if (n < 0) {
        n = -n;
    }
    else if (n == 0) {
        return 0;
    }
    return floorlog2(n);
}
static void Mfilter(int32_t * nsec_current, Filter * filt)
{
    int32_t s = 0, s2 = 0;
    /*
    using floatingpoint math
    alpha = 1/2^s
    y[1] = x[0]
    y[n] = alpha * x[n-1] + (1-alpha) * y[n-1]

    or equivalent with integer math
    y[1] = x[0]
    y_sum[1] = y[1] * 2^s
    y_sum[n] = y_sum[n-1] + x[n-1] - y[n-1]
    y[n] = y_sum[n] / 2^s
    */
    filt->n++; /* increment number of samples */
    /* 第一次的时候初始化y_prev 和 y_sum参数 */
    if (filt->n == 1) {
    		filt->y_prev = *nsec_current;
    		filt->y_sum = *nsec_current;
    		filt->s_prev = 0;
    }
    s = filt->s;
    /* speedup filter, if not 2^s > n */
    if ( (1 << s) > filt->n ) {
	/* lower the filter order */
		s = order(filt->n);
	} 
	else {
		/* avoid overflowing of n */
		filt->n = 1<<s;
	}
    /* avoid overflowing of filter. 30 is because using signed 32bit integers */
    s2 = 30 - order(max(filt->y_prev, *nsec_current));
    /* use the lower filter order, higher will overflow */
    s = min(s, s2);
    /* if the order of the filter changed, change also y_sum value */
    if(filt->s_prev > s) {
		filt->y_sum = filt->y_sum >> (filt->s_prev - s);
	} 
	else if (filt->s_prev < s) {
		filt->y_sum = filt->y_sum << (s - filt->s_prev);
	}
    /* compute the filter itself */
    filt->y_sum += *nsec_current - filt->y_prev;
    filt->y_prev = filt->y_sum >> s;
    /* save previous order of the filter */
    filt->s_prev = s;
    /* dbg("filter: %d -> %d (%d)\n", *nsec_current, filt->y_prev, s); */
    /* actualize target value */
    *nsec_current = filt->y_prev;
}

int buf1[200] ;

int buf2[200] ;

int main(void) {
    int input = 0, ra = 0, add = 0;
    int b = 0;
    MofM_filt.n = 0;				
    MofM_filt.s = 2;		/* 常用alpha 0.1-0.5，越小越激进，跟随性越差；0.5以后就非常保守了 */
    srand(time(NULL));
    int i = 0;
    for(i = 30; i < 130; i++) {
#if 0
        if(i > 70) {
            b = i/5 + 100;
        }else {
            b = i/5 ;

        }

#else 
    
     if(i > 80) {
            b = 2150 + i*12 ;
        }else {
            b = 2150 + i*19 ;

        }

      if(i ==  70) {
            b -= 1800;
        }else if(i ==  90){
            b -= 1000;

        }
#endif
        add = rand()%500 + b;
        input= i + add;
        ra = input;
        Mfilter(&input, &MofM_filt);
        //dbg("%d / %d\n", ra, input);
        buf1[i] = ra;
        buf2[i] = input;
    }



    for(i = 30; i < 130; i++) {
        dbg("%d.%d\n", buf1[i]/100, buf1[i]%100);
    }
    dbg("\n\n");
     for(i = 30; i < 130; i++) {
        dbg("%d.%d\n", buf2[i]/100, buf2[i]%100);
    }
}


